
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_compass.c
  * @author     baiyang
  * @date       2021-11-24
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sensor_compass.h"
#include "CompassCalibrator.h"
#include "sensor_compass_hmc5843.h"
#include "sensor_compass_lsm303d.h"
#include "sensor_compass_ist8310.h"
#include "sensor_compass_qmc5883l.h"

#include <float.h>
#include <stdio.h>
#include <rtconfig.h>

#include <device_manager/dev_mgr.h>
#include <device_manager/dev_mgr_i2c.h>
#include <board_config/board_config.h>
#include <common/console/console.h>
#include <parameter/param.h>
#include <gcs_mavlink/gcs.h>
/*-----------------------------------macro------------------------------------*/
#if COMPASS_MAX_UNREG_DEV > 0
#define CHECK_UNREG_LIMIT_RETURN  if (_sensor_compass._unreg_compass_count == COMPASS_MAX_UNREG_DEV) return
#else
#define CHECK_UNREG_LIMIT_RETURN
#endif

#ifndef COMPASS_GCS_SEND_TEXT
#define COMPASS_GCS_SEND_TEXT(severity, format, ...) gcs_send_text(severity, format, ##__VA_ARGS__)
#endif

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void _detect_backends(void);
static CompassStateIndex _get_state_id(CompassPriority priority);
static bool _have_i2c_driver(uint8_t bus, uint8_t address);
#if COMPASS_MAX_INSTANCES > 1
static void _reorder_compass_params();
#endif
static void _probe_external_i2c_compasses(void);
static bool _add_backend(sensor_compass_backend *backend);
static bool _driver_enabled(enum DriverType driver_type);
#if COMPASS_MAX_INSTANCES > 1 || COMPASS_MAX_UNREG_DEV
static CompassPriority _update_priority_list(int32_t dev_id);
#endif
static void try_set_initial_location();
static void _detect_runtime(void);
/*----------------------------------variable----------------------------------*/
static sensor_compass _sensor_compass;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/*
 * Get the sensor_baro singleton
 */
sensor_compass *sensor_compass_get_singleton()
{
    return &_sensor_compass;
}

// Default constructor.
// Note that the Vector/Matrix constructors already implicitly zero
// their values.
//
void sensor_compass_ctor(void)
{
    sensor_compass_assign_param();

    _sensor_compass._board_orientation = ROTATION_NONE;
    _sensor_compass._log_bit = -1;
}

void sensor_compass_init()
{
    if (!_sensor_compass._enabled) {
        return;
    }

#if COMPASS_MAX_INSTANCES > 1

    // Load priority list from storage, the changes to priority list
    // by user only take effect post reboot, after this
    for (CompassPriority i = 0; i<COMPASS_MAX_INSTANCES; i++) {
        if (_sensor_compass._priority_did_stored_list[i] != 0) {
            _sensor_compass._priority_did_list[i] = _sensor_compass._priority_did_stored_list[i];
        } else {
            // Maintain a list without gaps and duplicates
            for (CompassPriority j = i+1; j<COMPASS_MAX_INSTANCES; j++) {
                int32_t temp;
                if (_sensor_compass._priority_did_stored_list[j] == _sensor_compass._priority_did_stored_list[i]) {
                    sensor_compass_priority_did_stored_save_param(j, 0);
                }
                if (_sensor_compass._priority_did_stored_list[j] == 0) {
                    continue;
                }
                temp = _sensor_compass._priority_did_stored_list[j];
                sensor_compass_priority_did_stored_save_param(j, 0);
                _sensor_compass._priority_did_list[i] = temp;
                sensor_compass_priority_did_stored_save_param(i, temp);
                break;
            }
        }
    }
#endif // COMPASS_MAX_INSTANCES

    // cache expected dev ids for use during runtime detection
    for (CompassStateIndex i = 0; i<COMPASS_MAX_INSTANCES; i++) {
        _sensor_compass._state[i].expected_dev_id = _sensor_compass._state[i].dev_id;
    }

#if COMPASS_MAX_UNREG_DEV
    // set the dev_id to 0 for undetected compasses. extra_dev_id is just an
    // interface for users to see unreg compasses, we actually never store it
    // in storage.
    for (uint8_t i=_sensor_compass._unreg_compass_count; i<COMPASS_MAX_UNREG_DEV; i++) {
        // cache the extra devices detected in last boot
        // for detecting replacement mag
        _sensor_compass._previously_unreg_mag[i] = _sensor_compass.extra_dev_id[i];
        sensor_compass_extra_dev_id_save_param(i, 0);
    }
#endif

#if COMPASS_MAX_INSTANCES > 1
    // This method calls set_and_save_ifchanged on parameters
    // which are set() but not saved() during normal runtime,
    // do not move this call without ensuring that is not happening
    // read comments under set_and_save_ifchanged for details
    _reorder_compass_params();
#endif

    if (_sensor_compass._compass_count == 0) {
        // detect available backends. Only called once
        _detect_backends();
    }

#if COMPASS_MAX_UNREG_DEV
    // We store the list of unregistered mags detected here,
    // We don't do this during runtime, as we don't want to detect
    // compasses connected by user as a replacement while the system
    // is running
    for (uint8_t i=0; i<COMPASS_MAX_UNREG_DEV; i++) {
        sensor_compass_extra_dev_id_save_param2(i);
    }
#endif

    if (_sensor_compass._compass_count != 0) {
        // get initial health status
        rt_thread_mdelay(100);
        sensor_compass_read();
    }

    // set the dev_id to 0 for undetected compasses, to make it easier
    // for users to see how many compasses are detected. We don't do a
    // set_and_save() as the user may have temporarily removed the
    // compass, and we don't want to force a re-cal if they plug it
    // back in again
    for (CompassStateIndex i =0; i<COMPASS_MAX_INSTANCES; i++) {
        if (!_sensor_compass._state[i].registered) {
            sensor_compass_dev_id_set(i, 0);
        }
    }

#ifndef HAL_BUILD_AP_PERIPH
    // updating the AHRS orientation updates our own orientation:
    //AP::ahrs().update_orientation();
#endif

    _sensor_compass.init_done = true;
}

/*
  return true if we have a valid scale factor
 */
bool sensor_compass_have_scale_factor(uint8_t i)
{
    if (!(_sensor_compass._enabled && _sensor_compass.init_done)) {
        return false;
    }
    CompassStateIndex id = _get_state_id(i);
    if (id >= COMPASS_MAX_INSTANCES ||
        _sensor_compass._state[id].scale_factor < COMPASS_MIN_SCALE_FACTOR ||
        _sensor_compass._state[id].scale_factor > COMPASS_MAX_SCALE_FACTOR) {
        return false;
    }
    return true;
}

bool sensor_compass_read(void)
{
    if (!(_sensor_compass._enabled && _sensor_compass.init_done)) {
        return false;
    }

    if (!_sensor_compass._initial_location_set) {
        try_set_initial_location();
    }

    _detect_runtime();

    for (uint8_t i=0; i< _sensor_compass._backend_count; i++) {
        // call read on each of the backend. This call updates field[i]
        sensor_compass_backend_read(_sensor_compass._backends[i]);
    }

    uint32_t time = time_millis();
    bool any_healthy = false;
    for (CompassStateIndex i = 0; i < COMPASS_MAX_INSTANCES; i++) {
        _sensor_compass._state[i].healthy = (time - _sensor_compass._state[i].last_update_ms < 500);
        any_healthy |= _sensor_compass._state[i].healthy;
    }

#ifdef COMPASS_LEARN_ENABLED
    if (_sensor_compass._learn == LEARN_INFLIGHT && !_sensor_compass.learn_allocated) {
        _sensor_compass.learn_allocated = true;
    }
    if (_sensor_compass._learn == LEARN_INFLIGHT) {
    }
#endif

    // Set _first_usable parameter
    for (CompassPriority i = 0; i<COMPASS_MAX_INSTANCES; i++) {
        if (_sensor_compass._use_for_yaw[i]) {
            _sensor_compass._first_usable = i;
            break;
        }
    }

    return sensor_compass_healthy();
}

//  Register a new compass instance
//
bool sensor_compass_register_compass(int32_t dev_id, uint8_t* instance)
{

#if COMPASS_MAX_INSTANCES == 1 && !COMPASS_MAX_UNREG_DEV
    // simple single compass setup for AP_Periph
    CompassPriority priority = 0;
    CompassStateIndex i = 0;
    if (_sensor_compass._state[i].registered) {
        return false;
    }
    _sensor_compass._state[i].registered = true;
    _sensor_compass._state[i].priority = priority;
    *instance = i;
    _sensor_compass._compass_count = 1;
    return true;
#else
    CompassPriority priority;
    // Check if we already have this dev_id registered
    for (CompassStateIndex i = 0; i<COMPASS_MAX_INSTANCES; i++) {
        priority = _update_priority_list(dev_id);
        if (_sensor_compass._state[i].expected_dev_id == dev_id && priority < COMPASS_MAX_INSTANCES) {
            _sensor_compass._state[i].registered = true;
            _sensor_compass._state[i].priority = priority;
            *instance = i;
            return true;
        }
    }

    // This is an unregistered compass, check if any free slot is available
    for (CompassStateIndex i = 0; i<COMPASS_MAX_INSTANCES; i++) {
        priority = _update_priority_list(dev_id);
        if (_sensor_compass._state[i].dev_id == 0 && priority < COMPASS_MAX_INSTANCES) {
            _sensor_compass._state[i].registered = true;
            _sensor_compass._state[i].priority = priority;
            *instance = i;
            return true;
        }
    }

    // This might be a replacement compass module, find any unregistered compass
    // instance and replace that
    for (CompassStateIndex i = 0; i<COMPASS_MAX_INSTANCES; i++) {
        priority = _update_priority_list(dev_id);
        if (!_sensor_compass._state[i].registered && priority < COMPASS_MAX_INSTANCES) {
            _sensor_compass._state[i].registered = true;
            _sensor_compass._state[i].priority = priority;
            *instance = i;
            return true;
        }
    }
#endif

#if COMPASS_MAX_UNREG_DEV
    // Set extra dev id
    if (_sensor_compass._unreg_compass_count >= COMPASS_MAX_UNREG_DEV) {
        console_panic("Too many compass instances");
    }

    for (uint8_t i=0; i<COMPASS_MAX_UNREG_DEV; i++) {
        if (_sensor_compass.extra_dev_id[i] == dev_id) {
            if (i >= _sensor_compass._unreg_compass_count) {
                _sensor_compass._unreg_compass_count = i+1;
            }
            *instance = i+COMPASS_MAX_INSTANCES;
            return false;
        } else if (_sensor_compass.extra_dev_id[i] == 0) {
            sensor_compass_extra_dev_id_set_param(_sensor_compass._unreg_compass_count++, dev_id);
            *instance = i+COMPASS_MAX_INSTANCES;
            return false;
        }
    }
#else
    console_panic("Too many compass instances");
#endif
    return false;
}

// return last update time in microseconds
uint64_t sensor_compass_last_update_usec(void) { return sensor_compass_last_update_usec2(_sensor_compass._first_usable); }
uint64_t sensor_compass_last_update_usec2(uint8_t i) { return sensor_compass_get_state(i)->last_update_usec; }

uint32_t sensor_compass_last_update_ms(void) { return sensor_compass_last_update_ms2(_sensor_compass._first_usable); }
uint32_t sensor_compass_last_update_ms2(uint8_t i) { return sensor_compass_get_state(i)->last_update_ms; }

/// Return the current field as a Vector3f in milligauss
const Vector3f_t *sensor_compass_get_field2(uint8_t i) { return &(sensor_compass_get_state(i)->field); }
const Vector3f_t *sensor_compass_get_field(void) { return sensor_compass_get_field2(_sensor_compass._first_usable); }

/// Return the health of a compass
bool sensor_compass_healthy2(uint8_t i) { return (sensor_compass_get_state(i))->healthy; }
bool sensor_compass_healthy(void) { return sensor_compass_healthy2(_sensor_compass._first_usable); }

uint32_t sensor_compass_get_primary_id() { return (uint32_t)(sensor_compass_get_state(_sensor_compass._first_usable)->dev_id); }

// available returns true if the compass is both enabled and has
// been initialised
bool sensor_compass_available() { return _sensor_compass._enabled && _sensor_compass.init_done; }

/// return true if the compass should be used for yaw calculations
bool sensor_compass_use_for_yaw(void)
{
    return sensor_compass_healthy2(_sensor_compass._first_usable) && sensor_compass_use_for_yaw2(_sensor_compass._first_usable);
}

/// return true if the specified compass can be used for yaw calculations
bool sensor_compass_use_for_yaw2(uint8_t i)
{
    if (!sensor_compass_available()) {
        return false;
    }
    // when we are doing in-flight compass learning the state
    // estimator must not use the compass. The learning code turns off
    // inflight learning when it has converged
    return _sensor_compass._use_for_yaw[i] && _sensor_compass._learn != LEARN_INFLIGHT;
}

//Get State Struct by Priority
const mag_state* sensor_compass_get_state(CompassPriority priority)
{
    return &_sensor_compass._state[_get_state_id(priority)];
}

void sensor_compass_set_offsets(uint8_t i, const Vector3f_t* offsets)
{
    // sanity check compass instance provided
    CompassStateIndex id = _get_state_id((CompassPriority)i);

    if (id == 0) {
        param_set_obj(PARAM_ID(COMPASS, COMPASS_OFS_X), offsets->x);
        param_set_obj(PARAM_ID(COMPASS, COMPASS_OFS_Y), offsets->y);
        param_set_obj(PARAM_ID(COMPASS, COMPASS_OFS_Z), offsets->z);
    } else if (id == 1) {
        param_set_obj(PARAM_ID(COMPASS, COMPASS_OFS2_X), offsets->x);
        param_set_obj(PARAM_ID(COMPASS, COMPASS_OFS2_Y), offsets->y);
        param_set_obj(PARAM_ID(COMPASS, COMPASS_OFS2_Z), offsets->z);
    } else if (id == 2) {
        param_set_obj(PARAM_ID(COMPASS, COMPASS_OFS3_X), offsets->x);
        param_set_obj(PARAM_ID(COMPASS, COMPASS_OFS3_Y), offsets->y);
        param_set_obj(PARAM_ID(COMPASS, COMPASS_OFS3_Z), offsets->z);
    }
}

void sensor_compass_set_and_save_offsets(uint8_t i, const Vector3f_t* offsets)
{
    // sanity check compass instance provided
    CompassStateIndex id = _get_state_id((CompassPriority)i);
    if (id < COMPASS_MAX_INSTANCES) {
        sensor_compass_set_offsets(i, offsets);
        sensor_compass_save_offsets2(i);
    }
}

void sensor_compass_set_and_save_diagonals(uint8_t i, const Vector3f_t* diagonals)
{
    // sanity check compass instance provided
    CompassStateIndex id = _get_state_id((CompassPriority)i);

    if (id == 0) {
        param_set_and_save(PARAM_ID(COMPASS, COMPASS_DIA_X), diagonals->x);
        param_set_and_save(PARAM_ID(COMPASS, COMPASS_DIA_Y), diagonals->y);
        param_set_and_save(PARAM_ID(COMPASS, COMPASS_DIA_Z), diagonals->z);
    } else if (id == 1) {
        param_set_and_save(PARAM_ID(COMPASS, COMPASS_DIA2_X), diagonals->x);
        param_set_and_save(PARAM_ID(COMPASS, COMPASS_DIA2_Y), diagonals->y);
        param_set_and_save(PARAM_ID(COMPASS, COMPASS_DIA2_Z), diagonals->z);
    } else if (id == 2) {
        param_set_and_save(PARAM_ID(COMPASS, COMPASS_DIA3_X), diagonals->x);
        param_set_and_save(PARAM_ID(COMPASS, COMPASS_DIA3_Y), diagonals->y);
        param_set_and_save(PARAM_ID(COMPASS, COMPASS_DIA3_Z), diagonals->z);
    }
}

void sensor_compass_set_and_save_offdiagonals(uint8_t i, const Vector3f_t* offdiagonals)
{
    // sanity check compass instance provided
    CompassStateIndex id = _get_state_id((CompassPriority)i);

    if (id == 0) {
        param_set_and_save(PARAM_ID(COMPASS, COMPASS_ODI_X), offdiagonals->x);
        param_set_and_save(PARAM_ID(COMPASS, COMPASS_ODI_Y), offdiagonals->y);
        param_set_and_save(PARAM_ID(COMPASS, COMPASS_ODI_Z), offdiagonals->z);
    } else if (id == 1) {
        param_set_and_save(PARAM_ID(COMPASS, COMPASS_ODI2_X), offdiagonals->x);
        param_set_and_save(PARAM_ID(COMPASS, COMPASS_ODI2_Y), offdiagonals->y);
        param_set_and_save(PARAM_ID(COMPASS, COMPASS_ODI2_Z), offdiagonals->z);
    } else if (id == 2) {
        param_set_and_save(PARAM_ID(COMPASS, COMPASS_ODI3_X), offdiagonals->x);
        param_set_and_save(PARAM_ID(COMPASS, COMPASS_ODI3_Y), offdiagonals->y);
        param_set_and_save(PARAM_ID(COMPASS, COMPASS_ODI3_Z), offdiagonals->z);
    }
}

void sensor_compass_set_and_save_scale_factor(uint8_t i, float scale_factor)
{
    CompassStateIndex id = _get_state_id((CompassPriority)i);

    if (id == 0) {
        param_set_and_save(PARAM_ID(COMPASS, COMPASS_SCALE), scale_factor);
    } else if (id == 1) {
        param_set_and_save(PARAM_ID(COMPASS, COMPASS_SCALE2), scale_factor);
    } else if (id == 2) {
        param_set_and_save(PARAM_ID(COMPASS, COMPASS_SCALE3), scale_factor);
    }
}

void sensor_compass_save_offsets2(uint8_t i)
{
    CompassStateIndex id = _get_state_id((CompassPriority)i);
    if (id < COMPASS_MAX_INSTANCES) {
        sensor_compass_offset_save(i);
        sensor_compass_dev_id_set_and_save(i, _sensor_compass._state[id].detected_dev_id);
    }
}

void sensor_compass_set_and_save_orientation(uint8_t i, enum RotationEnum orientation)
{
    CompassStateIndex id = _get_state_id((CompassPriority)i);

    if (id == 0) {
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_ORIENT), orientation);
    } else if (id == 1) {
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_ORIENT2), orientation);
    } else if (id == 2) {
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_ORIENT3), orientation);
    }
}

void sensor_compass_save_offsets(void)
{
    for (CompassPriority i = 0; i<COMPASS_MAX_INSTANCES; i++) {
        sensor_compass_save_offsets2((uint8_t)i);
    }
}

void sensor_compass_reset_compass_id()
{
#if COMPASS_MAX_INSTANCES > 1
    // Check if any of the registered devs are not registered
    for (CompassPriority i = 0; i<COMPASS_MAX_INSTANCES; i++) {
        if (_sensor_compass._priority_did_stored_list[i] != _sensor_compass._priority_did_list[i] ||
            _sensor_compass._priority_did_stored_list[i] == 0) {
            //We don't touch priorities that might have been touched by the user
            continue;
        }
        if (!(sensor_compass_get_state(i))->registered) {
            sensor_compass_priority_did_stored_save_param(i, 0);
            COMPASS_GCS_SEND_TEXT(MAV_SEVERITY_ALERT, "Mag: Compass #%d with DEVID %lu removed", (uint8_t)(i), (unsigned long)_sensor_compass._priority_did_list[i]);
        }
    }

    // Check if any of the old registered devs are not registered
    // and hence can be removed
    for (CompassStateIndex i = 0; i<COMPASS_MAX_INSTANCES; i++) {
        if (_sensor_compass._state[i].dev_id == 0 && _sensor_compass._state[i].expected_dev_id != 0) {
            // also hard reset dev_ids that are not detected
            sensor_compass_dev_id_save(i);
        }
    }
#endif
}

//Convert StateIndex to Priority
CompassPriority sensor_compass_get_priority(CompassStateIndex state_id)
{
    return _sensor_compass._state[state_id].priority;
}

bool sensor_compass_cal_requires_reboot() { return _sensor_compass._cal_requires_reboot; }

// learn offsets accessor
bool sensor_compass_learn_offsets_enabled() { return _sensor_compass._learn == LEARN_INFLIGHT; }

bool sensor_compass_configured2(char *failure_msg, uint8_t failure_msg_len)
{
#if COMPASS_MAX_INSTANCES > 1
    // Check if any of the registered devs are not registered
    for (CompassPriority i = 0; i<COMPASS_MAX_INSTANCES; i++) {
        if (_sensor_compass._priority_did_list[i] != 0 && sensor_compass_use_for_yaw2(i)) {
            if (!sensor_compass_get_state(i)->registered) {
                snprintf(failure_msg, failure_msg_len, "Compass %d not found", (uint8_t)(i) + 1);
                return false;
            }
            if (_sensor_compass._priority_did_list[i] != _sensor_compass._priority_did_stored_list[i]) {
                snprintf(failure_msg, failure_msg_len, "Compass order change requires reboot");
                return false;
            }
        }
    }
#endif

    bool all_configured = true;
    for (uint8_t i=0; i<_sensor_compass._compass_count; i++) {
        if (sensor_compass_configured(i)) {
            continue;
        }
        if (!sensor_compass_use_for_yaw2(i)) {
            // we're not planning on using this anyway so sure,
            // whatever, it's configured....
            continue;
        }
        all_configured = false;
        break;
    }
    if (!all_configured) {
        snprintf(failure_msg, failure_msg_len, "Compass not calibrated");
    }
    return all_configured;
}

/// Returns True if the compasses have been configured (i.e. offsets saved)
///
/// @returns                    True if compass has been configured
///
bool sensor_compass_configured(uint8_t i)
{
    // exit immediately if instance is beyond the number of compasses we have available
    if (i > _sensor_compass._compass_count) {
        return false;
    }

    // exit immediately if all offsets are zero
    if (math_flt_zero(vec3_length(&(sensor_compass_get_state(i)->offset)))) {
        return false;
    }

    CompassStateIndex id = _get_state_id((CompassPriority)(i));
    // exit immediately if dev_id hasn't been detected
    if (_sensor_compass._state[id].detected_dev_id == 0 || 
        id == COMPASS_MAX_INSTANCES) {
        return false;
    }

    // back up cached value of dev_id
    int32_t dev_id_cache_value = _sensor_compass._state[id].dev_id;

    // if dev_id loaded from eeprom is different from detected dev id or dev_id loaded from eeprom is different from cached dev_id, compass is unconfigured
    if (_sensor_compass._state[id].dev_id != _sensor_compass._state[id].detected_dev_id || _sensor_compass._state[id].dev_id != dev_id_cache_value) {
        // restore cached value
        _sensor_compass._state[id].dev_id = dev_id_cache_value;
        // return failure
        return false;
    }

    // if we got here then it must be configured
    return true;
}

/// Returns the current offset values
///
/// @returns                    The current compass offsets in milligauss.
///
const Vector3f_t *sensor_compass_get_offsets2(uint8_t i) { return &(sensor_compass_get_state((CompassPriority)(i))->offset); }
const Vector3f_t *sensor_compass_get_offsets(void) { return &(sensor_compass_get_state(_sensor_compass._first_usable)->offset); }

// return maximum allowed compass offsets
uint16_t sensor_compass_get_offsets_max(void)
{
    return (uint16_t)_sensor_compass._offset_max;
}

bool sensor_compass_consistent()
{
    const Vector3f_t *primary_mag_field = sensor_compass_get_field();
    const Vector2f_t primary_mag_field_xy = {primary_mag_field->x,primary_mag_field->y};

    if (vec2_is_zero(&primary_mag_field_xy)) {
        return false;
    }

    Vector3f_t primary_mag_field_norm;
    vec3_norm(&primary_mag_field_norm, primary_mag_field);
    
    Vector2f_t primary_mag_field_xy_norm;
    vec2_norm(&primary_mag_field_xy_norm, &primary_mag_field_xy);

    for (uint8_t i=0; i<_sensor_compass._compass_count; i++) {
        if (!sensor_compass_use_for_yaw2(i)) {
            // configured not-to-be-used
            continue;
        }

        Vector3f_t mag_field = *(sensor_compass_get_field2(i));
        Vector2f_t mag_field_xy = {mag_field.x,mag_field.y};

        if (vec2_is_zero(&mag_field_xy)) {
            return false;
        }

        const float xy_len_diff  = sqrtf((primary_mag_field_xy.x - mag_field_xy.x) *(primary_mag_field_xy.x - mag_field_xy.x) + (primary_mag_field_xy.y - mag_field_xy.y) * (primary_mag_field_xy.y - mag_field_xy.y));

        vec3_norm(&mag_field, &mag_field);
        vec2_norm(&mag_field_xy, &mag_field_xy);

        const float xyz_ang_diff = acosf(math_constrain_float(vec3_dot(&mag_field, &primary_mag_field_norm),-1.0f,1.0f));
        const float xy_ang_diff  = acosf(math_constrain_float(vec2_dot(&mag_field_xy, &primary_mag_field_xy_norm),-1.0f,1.0f));

        // check for gross misalignment on all axes
        if (xyz_ang_diff > AP_COMPASS_MAX_XYZ_ANG_DIFF) {
            return false;
        }

        // check for an unacceptable angle difference on the xy plane
        if (xy_ang_diff > AP_COMPASS_MAX_XY_ANG_DIFF) {
            return false;
        }

        // check for an unacceptable length difference on the xy plane
        if (xy_len_diff > AP_COMPASS_MAX_XY_LENGTH_DIFF) {
            return false;
        }
    }
    return true;
}

/*
  macro to add a backend with check for too many backends or compass
  instances. We don't try to start more than the maximum allowed
 */
#define ADD_BACKEND(driver_type, backend)   \
    do { if (_driver_enabled(driver_type)) { _add_backend(backend); } \
        CHECK_UNREG_LIMIT_RETURN; \
    } while (0)

#define GET_I2C_DEVICE(bus, address) _have_i2c_driver(bus, address)?NULL:devmgr_get_i2c_device(bus, address)

/*
  detect available backends for this board
 */
static void _detect_backends(void)
{
#if GP_FEATURE_BOARD_DETECT
    if (brd_get_board_type() == PX4_BOARD_PIXHAWK2) {
        // default to disabling LIS3MDL on pixhawk2 due to hardware issue
        _sensor_compass._driver_type_mask = 1U<<COMPASS_DRIVER_LIS3MDL;
    }
#endif

    switch (brd_get_board_type()) {
    case PX4_BOARD_PX4V1:
    case PX4_BOARD_PIXHAWK:
    case PX4_BOARD_PHMINI:
    case PX4_BOARD_AUAV21:
    case PX4_BOARD_PH2SLIM:
    case PX4_BOARD_PIXHAWK2:
    case PX4_BOARD_MINDPXV2:
    case PX4_BOARD_FMUV5:
    case PX4_BOARD_FMUV6:
    case PX4_BOARD_PIXHAWK_PRO:
    case PX4_BOARD_AEROFC:
    case PX4_BOARD_AP0T:
    case PX4_BOARD_APGDSTV1:
    case PX4_BOARD_APGDSTV2:
    case PX4_BOARD_APGDSTV3:
    case MB_BOARD_REDROTORV1:
    case MB_BOARD_REOROTORV1:
        _probe_external_i2c_compasses();
        CHECK_UNREG_LIMIT_RETURN;
        break;

    default:
        break;
    }
    switch (brd_get_board_type()) {

#if defined(HAL_RTTHREAD_ARCH_FMUV2)
    case PX4_BOARD_PIXHAWK:
        ADD_BACKEND(COMPASS_DRIVER_HMC5843, sensor_compass_hmc5843_probe(devmgr_get_spi_device(HAL_COMPASS_HMC5843_NAME),
                    false, ROTATION_PITCH_180));
        ADD_BACKEND(COMPASS_DRIVER_LSM303D, sensor_compass_lsm303d_probe(devmgr_get_spi_device(LSM9DS0_SPI_DEVICE_NAME), ROTATION_NONE));
        break;
#endif

#if defined(HAL_RTTHREAD_ARCH_FMUV5) || \
    defined(HAL_RTTHREAD_ARCH_FMUV6)

    case PX4_BOARD_FMUV5:
    case PX4_BOARD_FMUV6:
        FOREACH_I2C_EXTERNAL(i) {
            ADD_BACKEND(COMPASS_DRIVER_IST8310, sensor_compass_ist8310_probe(GET_I2C_DEVICE(i, HAL_COMPASS_IST8310_I2C_ADDR),
                        true, ROTATION_ROLL_180_YAW_90));
        }
        FOREACH_I2C_INTERNAL(i) {
            ADD_BACKEND(COMPASS_DRIVER_IST8310, sensor_compass_ist8310_probe(GET_I2C_DEVICE(i, HAL_COMPASS_IST8310_I2C_ADDR),
                        false, ROTATION_ROLL_180_YAW_90));
        }
        break;
#endif

#if defined(HAL_RTTHREAD_ARCH_AP0T)
    case PX4_BOARD_AP0T:
        FOREACH_I2C_INTERNAL(i) {
            ADD_BACKEND(COMPASS_DRIVER_QMC5883L, sensor_compass_qmc5883l_probe(GET_I2C_DEVICE(i, HAL_COMPASS_QMC5883L_I2C_ADDR),
                        false,
                        ROTATION_YAW_90));
        }
        break;
#endif

#if defined(HAL_RTTHREAD_ARCH_APMCNV1)
    case PX4_BOARD_APMCNV1:
        FOREACH_I2C_INTERNAL(i) {
            ADD_BACKEND(COMPASS_DRIVER_QMC7983, sensor_compass_qmc5883l_probe(GET_I2C_DEVICE(i, HAL_COMPASS_QMC7983_I2C_ADDR),
                        false,
                        ROTATION_YAW_90));
        }
        break;
#endif

#if defined(HAL_RTTHREAD_ARCH_APGDSTV1) || \
    defined(HAL_RTTHREAD_ARCH_APGDSTV2) || \
    defined(HAL_RTTHREAD_ARCH_APGDSTV3)

    case PX4_BOARD_APGDSTV1:
    case PX4_BOARD_APGDSTV2:
    case PX4_BOARD_APGDSTV3:
        FOREACH_I2C_INTERNAL(i) {
            ADD_BACKEND(COMPASS_DRIVER_QMC5883L, sensor_compass_qmc5883l_probe(GET_I2C_DEVICE(i, HAL_COMPASS_QMC5883L_I2C_ADDR),
                        false,
                        ROTATION_ROLL_180_YAW_90));
            ADD_BACKEND(COMPASS_DRIVER_QMC7983, sensor_compass_qmc5883l_probe(GET_I2C_DEVICE(i, HAL_COMPASS_QMC7983_I2C_ADDR),
                        false,
                        ROTATION_ROLL_180_YAW_270));
        }
        break;
#endif

#if defined(HAL_RTTHREAD_ARCH_REDROTORV1)

    case MB_BOARD_REDROTORV1:
        FOREACH_I2C_INTERNAL(i) {
            ADD_BACKEND(COMPASS_DRIVER_QMC7983, sensor_compass_qmc5883l_probe(GET_I2C_DEVICE(i, HAL_COMPASS_QMC7983_I2C_ADDR),
                        false,
                        ROTATION_YAW_90));
        }
        break;
#endif

#if defined(HAL_RTTHREAD_ARCH_REOROTORV1)

    case MB_BOARD_REOROTORV1:
        FOREACH_I2C_INTERNAL(i) {
            ADD_BACKEND(COMPASS_DRIVER_QMC7983, sensor_compass_qmc5883l_probe(GET_I2C_DEVICE(i, HAL_COMPASS_QMC7983_I2C_ADDR),
                        false,
                        ROTATION_YAW_90));
        }
        break;
#endif

    default:
        break;
    }

    if (_sensor_compass._backend_count == 0 ||
        _sensor_compass._compass_count == 0) {
        console_printf("No Compass backends available\n");
    }
}

/*
  look for compasses on external i2c buses
 */
static void _probe_external_i2c_compasses(void)
{
    bool all_external = (brd_get_board_type() == PX4_BOARD_PIXHAWK2);

#if !defined(HAL_DISABLE_I2C_MAGS_BY_DEFAULT) || defined(HAL_USE_I2C_MAG_HMC5843)
    // external i2c bus
    FOREACH_I2C_EXTERNAL(i) {
        ADD_BACKEND(COMPASS_DRIVER_HMC5843, sensor_compass_hmc5843_probe(GET_I2C_DEVICE(i, HAL_COMPASS_HMC5843_I2C_ADDR),
                    true, ROTATION_ROLL_180));
    }

    if (brd_get_board_type() != PX4_BOARD_MINDPXV2 &&
        brd_get_board_type() != PX4_BOARD_AEROFC) {
        // internal i2c bus
        FOREACH_I2C_INTERNAL(i) {
            ADD_BACKEND(COMPASS_DRIVER_HMC5843, sensor_compass_hmc5843_probe(GET_I2C_DEVICE(i, HAL_COMPASS_HMC5843_I2C_ADDR),
                        all_external, all_external?ROTATION_ROLL_180:ROTATION_YAW_270));
        }
    }
#endif

#if !defined(HAL_DISABLE_I2C_MAGS_BY_DEFAULT) || defined(HAL_USE_I2C_MAG_QMC5883L)
    //external i2c bus
    FOREACH_I2C_EXTERNAL(i) {
        ADD_BACKEND(COMPASS_DRIVER_QMC5883L, sensor_compass_qmc5883l_probe(GET_I2C_DEVICE(i, HAL_COMPASS_QMC5883L_I2C_ADDR),
                    true, HAL_COMPASS_QMC5883L_ORIENTATION_EXTERNAL));
    }

    // internal i2c bus
    if (all_external) {
        // only probe QMC5883L on internal if we are treating internals as externals
        FOREACH_I2C_INTERNAL(i) {
            ADD_BACKEND(COMPASS_DRIVER_QMC5883L, sensor_compass_qmc5883l_probe(GET_I2C_DEVICE(i, HAL_COMPASS_QMC5883L_I2C_ADDR),
                        all_external,
                        all_external?HAL_COMPASS_QMC5883L_ORIENTATION_EXTERNAL:HAL_COMPASS_QMC5883L_ORIENTATION_INTERNAL));
        }
    }
#endif

#if defined(HAL_DISABLE_I2C_MAGS_BY_DEFAULT) || defined(HAL_USE_I2C_MAG_LIS3MDL)
    // lis3mdl on bus 0 with default address
    FOREACH_I2C_INTERNAL(i) {
        ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(GET_I2C_DEVICE(i, HAL_COMPASS_LIS3MDL_I2C_ADDR),
                    all_external, all_external?ROTATION_YAW_90:ROTATION_NONE));
    }

    // lis3mdl on bus 0 with alternate address
    FOREACH_I2C_INTERNAL(i) {
        ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(GET_I2C_DEVICE(i, HAL_COMPASS_LIS3MDL_I2C_ADDR2),
                    all_external, all_external?ROTATION_YAW_90:ROTATION_NONE));
    }

    // external lis3mdl on bus 1 with default address
    FOREACH_I2C_EXTERNAL(i) {
        ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(GET_I2C_DEVICE(i, HAL_COMPASS_LIS3MDL_I2C_ADDR),
                    true, ROTATION_YAW_90));
    }

    // external lis3mdl on bus 1 with alternate address
    FOREACH_I2C_EXTERNAL(i) {
        ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(GET_I2C_DEVICE(i, HAL_COMPASS_LIS3MDL_I2C_ADDR2),
                    true, ROTATION_YAW_90));
    }
#endif

#if !defined(HAL_DISABLE_I2C_MAGS_BY_DEFAULT) || defined(HAL_USE_I2C_MAG_IST8310)
    // IST8310 on external and internal bus
    if (brd_get_board_type() != PX4_BOARD_FMUV5 &&
        brd_get_board_type() != PX4_BOARD_FMUV6) {
        enum RotationEnum default_rotation;

        if (brd_get_board_type() == PX4_BOARD_AEROFC) {
            default_rotation = ROTATION_PITCH_180_YAW_90;
        } else {
            default_rotation = ROTATION_PITCH_180;
        }
        // probe all 4 possible addresses
        const uint8_t ist8310_addr[] = { 0x0C, 0x0D, 0x0E, 0x0F };

        for (uint8_t a=0; a<ARRAY_SIZE(ist8310_addr); a++) {
            FOREACH_I2C_EXTERNAL(i) {
                ADD_BACKEND(COMPASS_DRIVER_IST8310, sensor_compass_ist8310_probe(GET_I2C_DEVICE(i, ist8310_addr[a]),
                            true, default_rotation));
            }
            FOREACH_I2C_INTERNAL(i) {
                ADD_BACKEND(COMPASS_DRIVER_IST8310, sensor_compass_ist8310_probe(GET_I2C_DEVICE(i, ist8310_addr[a]),
                            all_external, default_rotation));
            }
        }
    }
#endif
}

static bool _add_backend(sensor_compass_backend *backend)
{
    if (!backend) {
        return false;
    }

    if (_sensor_compass._backend_count == COMPASS_MAX_BACKEND) {
        return false;
    }

    _sensor_compass._backends[_sensor_compass._backend_count++] = backend;

    return true;
}

/*
  return true if a driver type is enabled
 */
static bool _driver_enabled(enum DriverType driver_type)
{
    uint32_t mask = (1U<<(uint8_t)driver_type);
    return (mask & (uint32_t)_sensor_compass._driver_type_mask) == 0;
}

static CompassStateIndex _get_state_id(CompassPriority priority)
{
#if COMPASS_MAX_INSTANCES > 1
    if (_sensor_compass._priority_did_list[priority] == 0) {
        return COMPASS_MAX_INSTANCES;
    }
    for (CompassStateIndex i = 0; i<COMPASS_MAX_INSTANCES; i++) {
        if (_sensor_compass._priority_did_list[priority] == _sensor_compass._state[i].detected_dev_id) {
            return i;
        }
    }
    return COMPASS_MAX_INSTANCES;
#else
    return 0;
#endif
}

/*
  wrapper around hal.i2c_mgr->get_device() that prevents duplicate devices being opened
 */
static bool _have_i2c_driver(uint8_t bus, uint8_t address)
{
    for (CompassStateIndex i = 0; i<COMPASS_MAX_INSTANCES; i++) {
        if (!_sensor_compass._state[i].registered) {
            continue;
        }
        if (devmgr_make_bus_id(BUS_TYPE_I2C, bus, address, 0) ==
            devmgr_change_bus_id((uint32_t)_sensor_compass._state[i].dev_id, 0)) {
            // we are already using this device
            return true;
        }
    }
    return false;
}

#if COMPASS_MAX_INSTANCES > 1
// This method reorganises devid list to match
// priority list, only call before detection at boot
static void _reorder_compass_params()
{
    mag_state swap_state;
    CompassStateIndex curr_state_id;
    for (CompassPriority i = 0; i<COMPASS_MAX_INSTANCES; i++) {
        if (_sensor_compass._priority_did_list[i] == 0) {
            continue;
        }
        curr_state_id = COMPASS_MAX_INSTANCES;
        for (CompassStateIndex j = 0; j<COMPASS_MAX_INSTANCES; j++) {
            if (_sensor_compass._priority_did_list[i] == _sensor_compass._state[j].dev_id) {
                curr_state_id = j;
                break;
            }
        }
        if (curr_state_id != COMPASS_MAX_INSTANCES && curr_state_id != i) {
            //let's swa
            sensor_compass_mag_state_copy_from(&swap_state, curr_state_id, &_sensor_compass._state[curr_state_id]);
            sensor_compass_mag_state_copy_from(&_sensor_compass._state[curr_state_id], curr_state_id, &_sensor_compass._state[i]);
            sensor_compass_mag_state_copy_from(&_sensor_compass._state[i], i, &swap_state);
        }
    }
}
#endif

#if COMPASS_MAX_INSTANCES > 1 || COMPASS_MAX_UNREG_DEV
// Update Priority List for Mags, by default, we just
// load them as they come up the first time
static CompassPriority _update_priority_list(int32_t dev_id)
{
    // Check if already in priority list
    for (CompassPriority i = 0; i<COMPASS_MAX_INSTANCES; i++) {
        if (_sensor_compass._priority_did_list[i] == dev_id) {
            if (i >= _sensor_compass._compass_count) {
                _sensor_compass._compass_count = i+1;
            }
            return i;
        }
    }

    // We are not in priority list, let's add at first empty
    for (CompassPriority i = 0; i<COMPASS_MAX_INSTANCES; i++) {
        if (_sensor_compass._priority_did_stored_list[i] == 0) {
            sensor_compass_priority_did_stored_save_param(i, dev_id);
            _sensor_compass._priority_did_list[i] = dev_id;
            if (i >= _sensor_compass._compass_count) {
                _sensor_compass._compass_count = i+1;
            }
            return i;
        }
    }
    return COMPASS_MAX_INSTANCES;
}
#endif

/// Sets the initial location used to get declination
///
/// @param  latitude             GPS Latitude.
/// @param  longitude            GPS Longitude.
///
static void try_set_initial_location()
{
    if (!_sensor_compass._auto_declination) {
        return;
    }
    if (!_sensor_compass._enabled) {
        return;
    }

}

// Look for devices beyond initialisation
static void _detect_runtime(void)
{
#if HAL_ENABLE_LIBUAVCAN_DRIVERS

#endif
}

/*------------------------------------test------------------------------------*/


